#include "QuadrupedCreature.h"
namespace jcrada {

    QuadrupedCreature::QuadrupedCreature( int hidden_layers, int neurons_per_layer )
    : Creature( BODY_PARTS, CONSTRAINTS, hidden_layers, neurons_per_layer ) {
        setName("QuadrupedCreature");
    }

    QuadrupedCreature::~QuadrupedCreature( ) {
    }

    void QuadrupedCreature::set( const btTransform& at ) {
        getBodyPart(ROOT).setId(ROOT);
        getBodyPart(ROOT).setName("Root");
        getBodyPart(ROOT).setParentId(Creature::B_NONE);
        getBodyPart(ROOT).setShape(BodyPart::S_CAPSULE_Z);
        getBodyPart(ROOT).setSize(0.75, 0.75, 2.0);
        getBodyPart(ROOT).setMass(2.0);

        getBodyPart(FLLEG).setId(FLLEG);
        getBodyPart(FLLEG).setName("FrontLeftLeg");
        getBodyPart(FLLEG).setParentId(ROOT);
        getBodyPart(FLLEG).setShape(BodyPart::S_CAPSULE_Y);
        getBodyPart(FLLEG).setSize(0.25, 1.0, 0.25);
        getBodyPart(FLLEG).setMass(1.0);

        getBodyPart(FRLEG).setId(FRLEG);
        getBodyPart(FRLEG).setName("FrontRightLeg");
        getBodyPart(FRLEG).setParentId(ROOT);
        getBodyPart(FRLEG).setShape(BodyPart::S_CAPSULE_Y);
        getBodyPart(FRLEG).setSize(0.25, 1.0, 0.25);
        getBodyPart(FRLEG).setMass(1.0);

        getBodyPart(BLLEG).setId(BLLEG);
        getBodyPart(BLLEG).setName("BackLeftLeg");
        getBodyPart(BLLEG).setParentId(ROOT);
        getBodyPart(BLLEG).setShape(BodyPart::S_CAPSULE_Y);
        getBodyPart(BLLEG).setSize(0.25, 1.0, 0.25);
        getBodyPart(BLLEG).setMass(1.0);

        getBodyPart(BRLEG).setId(BRLEG);
        getBodyPart(BRLEG).setName("BackRightLeg");
        getBodyPart(BRLEG).setParentId(ROOT);
        getBodyPart(BRLEG).setShape(BodyPart::S_CAPSULE_Y);
        getBodyPart(BRLEG).setSize(0.25, 1.0, 0.25);
        getBodyPart(BRLEG).setMass(1.0);

        for (int i=  0 ; i < CONSTRAINTS ;++i){
            getConstraint(i).setType(Constraint::CT_GENERIC_6DOF);
        }

        getConstraint(FLHIP).setId(FLHIP);
        getConstraint(FLHIP).setName("FrontLeftHip");
        getConstraint(FLHIP).setIdBodyA(getBodyPart(ROOT).getId());
        getConstraint(FLHIP).setIdBodyB(getBodyPart(FLLEG).getId());
        getConstraint(FLHIP).setConnectionA(Constraint::CYLINDER_R5_LEFT);
        getConstraint(FLHIP).setConnectionB(Constraint::CYLINDER_TOP);
        getConstraint(FLHIP).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
        getConstraint(FLHIP).setAngularLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(M_PI_4, M_PI_4, M_PI_4));

        getConstraint(FRHIP).setId(FRHIP);
        getConstraint(FRHIP).setName("FrontRightHip");
        getConstraint(FRHIP).setIdBodyA(getBodyPart(ROOT).getId());
        getConstraint(FRHIP).setIdBodyB(getBodyPart(FRLEG).getId());
        getConstraint(FRHIP).setConnectionA(Constraint::CYLINDER_R5_RIGHT);
        getConstraint(FRHIP).setConnectionB(Constraint::CYLINDER_TOP);
        getConstraint(FRHIP).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
        getConstraint(FRHIP).setAngularLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(M_PI_4, M_PI_4, M_PI_4));

        getConstraint(BLHIP).setId(BLHIP);
        getConstraint(BLHIP).setName("BackLeftHip");
        getConstraint(BLHIP).setIdBodyA(getBodyPart(ROOT).getId());
        getConstraint(BLHIP).setIdBodyB(getBodyPart(BLLEG).getId());
        getConstraint(BLHIP).setConnectionA(Constraint::CYLINDER_R1_LEFT);
        getConstraint(BLHIP).setConnectionB(Constraint::CYLINDER_TOP);
        getConstraint(BLHIP).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
        getConstraint(BLHIP).setAngularLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(M_PI_4, M_PI_4, M_PI_4));

        getConstraint(BRHIP).setId(BRHIP);
        getConstraint(BRHIP).setName("BackRightHip");
        getConstraint(BRHIP).setIdBodyA(getBodyPart(ROOT).getId());
        getConstraint(BRHIP).setIdBodyB(getBodyPart(BRLEG).getId());
        getConstraint(BRHIP).setConnectionA(Constraint::CYLINDER_R1_RIGHT);
        getConstraint(BRHIP).setConnectionB(Constraint::CYLINDER_TOP);
        getConstraint(BRHIP).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
        getConstraint(BRHIP).setAngularLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(M_PI_4, M_PI_4, M_PI_4));

        Creature::set(at);
    }

}

